#ifndef LOCALOPTIM_H
#define LOCALOPTIM_H

#include "KeyPoseFrame.h"
#include "Atlas.h"
#include "Tracking.h"
#include "Settings.h"

#include <list>
#include <mutex>

namespace msf
{
class System;
class Tracking;
class Atlas;

class LocalOptimizing
{
public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
      LocalOptimizing(System* pSys, Atlas* pAtlas, bool bInertial);

      void SetTracker(Tracking* pTracker);

      // Main函数
      void Run();

      void InsertKeyPoseFrame(KeyPoseFrame* pKPF);
      void EmptyQueue();

      // 线程同步
      void RequestStop();
      void RequestReset();
      void RequestResetActiveMap(Map* pMap);
      bool Stop();
      void Release();
      bool isStopped();
      bool stopRequested();
      bool AcceptKeyPoseFrames();
      void SetAcceptKeyPoseFrames(bool flag);
      bool SetNotStop(bool flag);

      void InterruptBA();

      void RequestFinish();
      bool isFinished();

      int KeyposeframesInQueue(){
            std::unique_lock<std::mutex> lock(mMutexNewKPFs);
            return mlNewKeyPoseFrames.size();
      }

      bool IsInitializing();
      double GetCurrKPFTime();
      KeyPoseFrame* GetCurrKPF();

      std::mutex mMutexImuInit;

      Eigen::MatrixXd mcovInertial;
      Eigen::Matrix3d mRwg;
      Eigen::Vector3d mbg;
      Eigen::Vector3d mba;
      double mScale;
      double mInitTime;
      double mCostTime;

      unsigned int mInitSect;
      unsigned int mIdxInit;
      unsigned int mnKPFs;
      double mFirstTs;

      // For debugging (erase in normal mode)
      int mInitFr;

      bool mbNotBA1;
      bool mbNotBA2;
      bool mbBadImu;

protected:

      bool CheckNewKeyPoseFrames();
      void ProcessNewKeyPoseFrame();

      void SearchInNeighbors();
      void KeyPoseFrameCulling();

      System *mpSystem;

      bool mbInertial;

      void ResetIfRequested();
      bool mbResetRequested;
      bool mbResetRequestedActiveMap;
      Map* mpMapToReset;
      std::mutex mMutexReset;

      bool CheckFinish();
      void SetFinish();
      bool mbFinishRequested;
      bool mbFinished;
      std::mutex mMutexFinish;

      Atlas* mpAtlas;

      Tracking* mpTracker;

      std::list<KeyPoseFrame*> mlNewKeyPoseFrames;

      KeyPoseFrame* mpCurrentKeyPoseFrame;

      std::mutex mMutexNewKPFs;

      bool mbAbortBA;     

      bool mbStopped;
      bool mbStopRequested;
      bool mbNotStop;
      std::mutex mMutexStop;

      bool mbAcceptKeyPoseFrames;
      std::mutex mMutexAccept;

      void InitializeIMU(float priorG = 1e2, float priorA = 1e6, bool bFirst = false);
      void ScaleRefinement();

      bool bInitializing;

      Eigen::MatrixXd infoInertial;
      int mNumLM;
      int mNumKPFCulling;

      float mTinit;

      int countRefinement;
};

} //namespace msf

#endif //LOCALOPTIM_H